#include "legs.h"
#include "SoR_Utils.h"

#include "legs_data.h"
void move_leg_part(int* leg_part, int pos, int duration) {
	int i;
//	unsigned int port_number;
//	unsigned int port_letter;
//	int callibration_data;
	for (i=0;i<15;i++) { 
		servo(
			// Finds the port letter
			*(leg_port_letters[leg_part[0]][leg_part[1]][leg_part[2]]), 
			// Finds the port number
			leg_port_numbers[leg_part[0]][leg_part[1]][leg_part[2]], 
			// Calculates the position to move, based on the calibration matrix
			abs(leg_calibration[leg_part[0]][leg_part[1]][leg_part[2]] + pos)); 
			rprintf("%d\n", abs(leg_calibration[leg_part[0]][leg_part[1]][leg_part[2]] + pos)); 
			rprintf("%d %d %d\n",leg_part[0],leg_part[1],leg_part[2]);
		delay_ms(30); 
	} 
}

void center_all_legs() {
			execute_command("uflc");
			execute_command("ufrc");
			execute_command("ublc");
			execute_command("ubrc");
			execute_command("lflc");
			execute_command("lfrc");
			execute_command("lblc");
			execute_command("lbrc");
}

void walk() {
	int i;
	for (i=0; i<16; i++) {
		move_leg_part(walking_sequence[i], walking_sequence[i][3],0);
	}
}

void execute_command(char* command) {
	// The recogined commands are of the form
	// abc<position> 
	//   or
	// abc<number>
	// The First three bytes are the position of the leg component
	// Remaining bytes are the position to which to move that component,
	// either: f (front), b (back), c (center); or a number 
	// representing the angular distance from the center position
	unsigned int cmd[4];
	int pos;
	int i,j;
	int command_length;
	int arg = atoi(command+3);
	// If command+3 is a valid number, arg will be <> 0
	// and we do not need to parse the fourth char of the command string
	if (arg) {
		command_length=3;
	} else {
		command_length=4;
	}


	// Convert commands in the first three chars into the equivalent numbers
	// representing the position of the leg component
	for (i=0; i<command_length; i++) {
		for (j=0; j<3; j++) {
			cmd[i]=j;
			if (leg_comp_pos[i][j] == command[i]) break;
		}
		if (j>=3) {
			rprintf("COMMAND NOT RECOGNIZED\n");
			return;
		}
	}
	if (arg) {
		pos = arg;
	} else {
		pos = positions[cmd[3]];
	}

	move_leg_part(cmd, pos, 0);

}




char read_byte() {
	char byte = 255;
	while (byte == 255) {
		byte = uart1GetByte();
	}
	uartFlushReceiveBuffer(1);
	return byte;
}

void read_command(char* command) {
	char byte = read_byte();
		
	int j=0;
	while ((byte != '\n') && (byte != ' ')) {
		command[j]=byte;
		j++;
		byte = read_byte();
	}
	command[j]=0;
}

void rprintf_str(char* str) {
	int j=0;
	while (str[j] != 0) {
		rprintf("%c", str[j]);
		j++;
	}
}
